#include "Matrix3x3.h"
#include "MathBase.h"

SMatrix3x3 CreateZero()
{
    SMatrix3x3 Ret;
    Ret.Data[0][0] = 0;
    Ret.Data[0][1] = 0;
    Ret.Data[0][2] = 0;

    Ret.Data[1][0] = 0;
    Ret.Data[1][1] = 0;
    Ret.Data[1][2] = 0;

    Ret.Data[2][0] = 0;
    Ret.Data[2][1] = 0;
    Ret.Data[2][2] = 0;

    return Ret;
}

SMatrix3x3 SMatrix3x3::Zero = CreateZero();

CString SMatrix3x3::ToString() const
{
    return U"Matrix3x3:[\n"
    +CString(Data[0][0])+U","+CString(Data[1][0])+U","+CString(Data[2][0])+U"\n"
    +CString(Data[0][1])+U","+CString(Data[1][1])+U","+CString(Data[2][1])+U"\n"
    +CString(Data[0][2])+U","+CString(Data[1][2])+U","+CString(Data[2][2])+U"\n"
    +U"]";
}

void SMatrix3x3::ToFloatArray(float OutArray[9]) const
{
    OutArray[0] = Data[0][0];
    OutArray[1] = Data[0][1];
    OutArray[2] = Data[0][2];

    OutArray[3] = Data[1][0];
    OutArray[4] = Data[1][1];
    OutArray[5] = Data[1][2];

    OutArray[6] = Data[2][0];
    OutArray[7] = Data[2][1];
    OutArray[8] = Data[2][2];
}

SMatrix3x3 SMatrix3x3::Inverse() const
{


    Float Det=Data[0][0]*(Data[1][1]*Data[2][2]-Data[1][2]*Data[2][1])
             -Data[0][1]*(Data[1][0]*Data[2][2]-Data[1][2]*Data[2][0])
             +Data[0][2]*(Data[1][0]*Data[2][1]-Data[1][1]*Data[2][0]);

    if(Det==0)
    {
        return SMatrix3x3::Identity();
    }

    SMatrix3x3 Ret;

    Float InvDet=1.f/Det;

    Ret.Data[0][0]=(Data[1][1]*Data[2][2]-Data[1][2]*Data[2][1])*InvDet;
    Ret.Data[0][1]=-(Data[0][1]*Data[2][2]-Data[0][2]*Data[2][1])*InvDet;
    Ret.Data[0][2]=(Data[0][1]*Data[1][2]-Data[0][2]*Data[1][1])*InvDet;

    Ret.Data[1][0]=-(Data[1][0]*Data[2][2]-Data[1][2]*Data[2][0])*InvDet;
    Ret.Data[1][1]=(Data[0][0]*Data[2][2]-Data[0][2]*Data[2][0])*InvDet;
    Ret.Data[1][2]=-(Data[0][0]*Data[1][2]-Data[0][2]*Data[1][0])*InvDet;

    Ret.Data[2][0]=(Data[1][0]*Data[2][1]-Data[1][1]*Data[2][0])*InvDet;
    Ret.Data[2][1]=-(Data[0][0]*Data[2][1]-Data[0][1]*Data[2][0])*InvDet;
    Ret.Data[2][2]=(Data[0][0]*Data[1][1]-Data[0][1]*Data[1][0])*InvDet;

    return Ret;


}

SMatrix3x3 SMatrix3x3::Transpose() const
{
    SMatrix3x3 Ret;

    Ret.Data[0][0]=Data[0][0];
    Ret.Data[0][1]=Data[1][0];
    Ret.Data[0][2]=Data[2][0];

    Ret.Data[1][0]=Data[0][1];
    Ret.Data[1][1]=Data[1][1];
    Ret.Data[1][2]=Data[2][1];

    Ret.Data[2][0]=Data[0][2];
    Ret.Data[2][1]=Data[1][2];
    Ret.Data[2][2]=Data[2][2];

    return Ret;
}

SMatrix3x3 SMatrix3x3::FromEuler(const SVec3& InRandian)
{
	// Matrix= cosZ  -sinZ 0            cosY     0     sinY             1     0     0
	// 		   sinZ cosZ 0      *       0        1     0          *     0     cosX  -sinX
	// 		   0     0    1            -sinY    0     cosY             0     sinX  cosX

	// = cZ      -sZ    cZsY         1  0   0
    //   sZcY    cZ     sZsY   *    0  cX  -sX
    //   -sY     0     cY           0  sX  cX

	// =  cZcY      -sZcX+cZsYsX  sZsX+cZsYcX
	// 	 sZcY     cZcX+sZsYsX   -cZsX +sZsYcX
	// 	 -sY       cYsX           cYcX 

    auto cX=CMathBase::Cos(InRandian.X) ;
    auto sX=CMathBase::Sin(InRandian.X) ;

    auto cY=CMathBase::Cos(InRandian.Y) ;
    auto sY=CMathBase::Sin(InRandian.Y) ;  

    auto cZ=CMathBase::Cos(InRandian.Z) ;
    auto sZ=CMathBase::Sin(InRandian.Z) ;

    SMatrix3x3 Matrix;

	Matrix.Data[0][0] = cZ* cY;
	Matrix.Data[0][1] = sZ*cY;
	Matrix.Data[0][2] = -sY;

	Matrix.Data[1][0] = -sZ*cX + cZ*sY *sX;
	Matrix.Data[1][1] = cZ *cX +sZ*sY *sX;
	Matrix.Data[1][2] = cY * sX;	

	Matrix.Data[2][0] = sZ*sX + cZ *sY * cX;
	Matrix.Data[2][1] = -cZ * sX +sZ * sY *cX;
	Matrix.Data[2][2] = cY *cX;	

    return Matrix;
}

SVec3 SMatrix3x3::ToEuler() const
{
	SVec3 Euler;

	Euler.X= CMathBase::Atan(Data[2][2]/Data[1][2]);
	Euler.Y= CMathBase::Asin(-Data[0][2]);
	Euler.Z= CMathBase::Atan(Data[0][1]/Data[0][0]);   

    return  Euler;

}

SMatrix3x3::SMatrix3x3(const SQuat& InQuat)
{
    const SQuat& Q = InQuat;
    Data[0][0] = 1 - 2 * Q.Y * Q.Y - 2 * Q.Z * Q.Z;
    Data[1][0] = 2 * Q.X * Q.Y - 2 * Q.W * Q.Z;
    Data[2][0] = 2 * Q.X * Q.Z + 2 * Q.W * Q.Y;

    Data[0][1] = 2 * Q.X * Q.Y + 2 * Q.W * Q.Z;
    Data[1][1] = 1 - 2 * Q.X * Q.X - 2 * Q.Z * Q.Z;
    Data[2][1] = 2 * Q.Y * Q.Z - 2 * Q.W * Q.X;

    Data[0][2] = 2 * Q.X * Q.Z - 2 * Q.W * Q.Y;
    Data[1][2] = 2 * Q.Y * Q.Z + 2 * Q.W * Q.X;
    Data[2][2] = 1 - 2 * Q.X * Q.X - 2 * Q.Y * Q.Y;
}

// glm::mat3x3 SMatrix3x3::ToGlm() const
// {
//     glm::mat3x3 Ret;
//     for (int Row = 0; Row < 3; Row++)
//     {
//         for (int Column = 0; Column < 3; Column++)
//         {
//             Ret[Column][Row] = Data[Column][Row];
//         }
//     }
//     return Ret;
// }

SQuat SMatrix3x3::ToQuat() const
{
    Float Tr=Data[0][0]+Data[1][1]+Data[2][2];
    if(Tr>0)
    {
        SQuat Ret;

        Ret.W= CMathBase::Sqrt(Tr+1)*0.5f;
        Float W4=0.25f/Ret.W;
        Ret.X= (Data[1][2]-Data[2][1])*W4;
        Ret.Y= (Data[2][0]-Data[0][2])*W4;
        Ret.Z= (Data[0][1]-Data[1][0])*W4;

        return Ret;

    }

    if(Data[0][0]>=Data[1][1] &&Data[0][0]>=Data[2][2]  )
    {
        SQuat Ret;

        Ret.X= CMathBase::Sqrt(Data[0][0]-Data[1][1]-Data[2][2]+1)/2;
        Float X4=0.25f/Ret.X;
        Ret.Y= (Data[0][1]+Data[1][0])*X4;
        Ret.Z= (Data[0][2]+Data[2][0])*X4;
        Ret.W= (Data[1][2]-Data[2][1])*X4;

        return Ret;
    }
    else if(Data[1][1]>=Data[0][0] &&Data[1][1]>=Data[2][2]  )
    {
        SQuat Ret;

        Ret.Y= CMathBase::Sqrt(Data[1][1]-Data[0][0]-Data[2][2]+1)/2;
        Float Y4=0.25f/Ret.Y;
        Ret.X= (Data[0][1]+Data[1][0])*Y4;
        Ret.Z= (Data[1][2]+Data[2][1])*Y4;
        Ret.W= (Data[2][0]-Data[0][2])*Y4;

        return Ret;
    }
    else //if(Data[2][2]>=Data[0][0] &&Data[2][2]>=Data[0][0]  )
    {
        SQuat Ret;

        Ret.Z= CMathBase::Sqrt(Data[2][2]-Data[0][0]-Data[1][1]+1)/2;
        Float Z4=0.25f/Ret.Z;
        Ret.X= (Data[0][2]+Data[2][0])*Z4;
        Ret.Y= (Data[1][2]+Data[2][1])*Z4;
        Ret.W= (Data[0][1]-Data[1][0])*Z4;

        return Ret;        
    }


}